//
// Created by wangjunjun on 2024/3/25.
//

#ifndef MESSAGE_LCM_HANDLER_H
#define MESSAGE_LCM_HANDLER_H
#include "channel/leg_control_command_lcmt.hpp"
#include "channel/leg_control_data_lcmt.hpp"
#include "channel/microstrain_lcmt.hpp"
#include "channel/state_estimator_lcmt.hpp"
#include "channel/udp_data_lcm_t.hpp"
#include "lcm/lcm-cpp.hpp"
#include "../util/generic.h"
#include "../log/log.h"
#include <mutex>
#include <memory>

struct ImuDataEntry {
    float rpy[3];
    float omega[3];
    float acc[3];
};

struct StateEstData {
    float p[3];
    float rpy[3];
    float omegaBody[3];
    int32_t power;
};
struct OdomEstData {
    float p[3];
    float rpy[3];
    float omegaBody[3];
};
struct LegControlData{
    float      q[12];
    float      qd[12];
    float      p[12];
    float      v[12];
};
struct LegControlCommand{
    float      tau_ff[12];
    float      f_ff[12];
    float      q_des[12];
    float      qd_des[12];
    float      p_des[12];
    float      v_des[12];
    float      kp_cartesian[12];
    float      kd_cartesian[12];
    float      kp_joint[12];
    float      kd_joint[12];
};
struct UdpData{
    float      p_des[2];
    float      v_des[3];
    float      rpy_des[3];
    float      omega_des[3];
    float      mode;
};
class CMessageLcmHandler{
public:
    //设置单例模式
    static  std::shared_ptr<CMessageLcmHandler> GetInStance();
    void HandlerImuData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const micro_lcmt::microstrain_lcmt* msg);
    void HandlerStateData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const state_lcmt::state_estimator_lcmt* msg);
    void HandlerLegData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const leg_control_data_lcmt* msg);
    void HandlerLegCommandData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const channel::leg_control_command_lcmt* msg);
    void HandlerUdpData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const udp_data_lcm_t* msg);
    void SaveSigDataToFile();
    //获取状态
    float GetControlCmdStatus();
    //获取电量
    float GetPowerStatus();
    //启动新线程执行LCM
    void Start();
private:
    CMessageLcmHandler();
    ~CMessageLcmHandler();

    //LCM 处理线程
    void HandlerLcmProc(void);

    //保存数据到文本文件方法

private:
    std::mutex dataLock;
    ImuDataEntry imuData;
    StateEstData stateEstData;

    LegControlData legControlData;
    LegControlCommand legControlCommand;
    UdpData udpData;
    static std::shared_ptr<CMessageLcmHandler> m_Instance;
    static std::once_flag m_flag;

    std::shared_ptr<CGeneric> m_pGenptr;
};


#endif //MESSAGE_LCM_HANDLER_H
